/**
  ****************************(C) COPYRIGHT 2024 Polarbear*************************
  * @file       usb_task.c/h
  * @brief      通过USB串口与上位机通信
  * @note
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Jun-24-2024     Penguin         1. done

  @verbatim
  =================================================================================

  =================================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2024 Polarbear*************************
*/

#include "usb_task.h"

#include <stdbool.h>
#include <string.h>

#include "CRC8_CRC16.h"
#include "cmsis_os.h"
#include "data_exchange_center.h"
#include "macro_typedef.h"
#include "usb_device.h"
#include "usb_typdef.h"
#include "usbd_cdc_if.h"
#include "usbd_conf.h"
#include "referee.h"
#include "vision_task.h"



#if INCLUDE_uxTaskGetStackHighWaterMark
uint32_t usb_high_water;
#endif

#define USB_TASK_CONTROL_TIME 1  // ms

#define USB_OFFLINE_THRESHOLD 100  // ms
#define USB_CONNECT_CNT 10

// clang-format off
// #define SEND_DURATION_Imu         5   // ms
// #define SEND_DURATION_Debug       5   // ms
#define SEND_DURATION_RobotInfo   10  // ms
// #define SEND_DURATION_Pid         10  // ms
#define SEND_DURATION_AllRobotHp  10  // ms
#define SEND_DURATION_GameStatus  10  // ms
// #define SEND_DURATION_RobotMotion 10  // ms
#define SEND_DURATION_Vision       10  // ms
#define SEND_DURATION_Simulation   10  // ms
// clang-format on

#define USB_RX_DATA_SIZE 256  // byte
#define USB_RECEIVE_LEN 200   // byte
#define HEADER_SIZE 4         // byte
#define ROUND_TO_4DEC(v) (roundf((v) * 10000.0f) / 10000.0f)
#define CheckDurationAndSend(send_name)                                                  \
    do {                                                                                 \
        if ((HAL_GetTick() - LAST_SEND_TIME.##send_name) >= SEND_DURATION_##send_name) { \
            LAST_SEND_TIME.##send_name = HAL_GetTick();                                  \
            UsbSend##send_name##Data();                                                  \
        }                                                                                \
    } while (0)

    


// Variable Declarations
static uint8_t USB_RX_BUF[USB_RX_DATA_SIZE];



Imu_t * IMU;
SolveAns* solveAns;

// static const ChassisSpeedVector_t * FDB_SPEED_VECTOR;

// 判断USB连接状态用到的一些变量
static bool USB_OFFLINE = true;
static uint32_t RECEIVE_TIME = 0;
static uint32_t LATEST_RX_TIMESTAMP = 0;
static uint32_t CONTINUE_RECEIVE_CNT = 0;

// 数据发送结构体
// clang-format off
// static SendDataDebug_s       SEND_DATA_DEBUG;
// static SendDataImu_s         SEND_DATA_IMU;
static SendDataRobotInfo_s      SEND_DATA_ROBOT_INFO;
// static SendDataPidDebug_s    SEND_DATA_PID;
static SendDataAllRobotHp_s     SEND_DATA_ALL_ROBOT_HP;
static SendDataGameStatus_s     SEND_DATA_GAME_STATUS;
// static SendDataRobotMotion_s SEND_ROBOT_MOTION_DATA;
static SendDataVision_s         SEND_DATA_VISION;
// clang-format on

// 数据接收结构体
static ReceiveDataRobotCmd_s    RECEIVE_ROBOT_CMD_DATA;
// static ReceiveDataPidDebug_s    RECEIVE_PID_DEBUG_DATA;
// static ReceiveDataVirtualRc_s   RECEIVE_VIRTUAL_RC_DATA;
static ReceiveDataVision_s      RECEIVE_VISION_DATA;
static ReceiveDataSimulation_s  RECEIVE_SIMULATION_DATA;

// 机器人控制指令数据
// RobotCmdData_t ROBOT_CMD_DATA;
// static RC_ctrl_t VIRTUAL_RC_CTRL;

// 发送数据间隔时间
typedef struct
{
    // uint32_t Imu;
    // uint32_t Debug;
    uint32_t RobotInfo;
    // uint32_t Pid;
    uint32_t AllRobotHp;
    uint32_t GameStatus;
    // uint32_t RobotMotion;
    uint32_t Vision;
} LastSendTime_t;
static LastSendTime_t LAST_SEND_TIME;

/*******************************************************************************/
/* Main Function                                                               */
/*******************************************************************************/

static void UsbSendData(void);
static void UsbReceiveData(void);
static void UsbInit(void);

/*******************************************************************************/
/* Send Function                                                               */
/*******************************************************************************/

// static void UsbSendImuData(void);
// static void UsbSendDebugData(void);
static void UsbSendRobotInfoData(void);
static void UsbSendAllRobotHpData(void);
static void UsbSendGameStatusData(void);
// static void UsbSendRobotMotionData(void);
static void UsbSendVisionData(void);

/*******************************************************************************/
/* Receive Function                                                            */
/*******************************************************************************/

ReceiveDataRobotCmd_s* GetRobotCmdData(void);
// static void GetVirtualRcCtrlData(void);
ReceiveDataVision_s* GetVisionData(void);
ReceiveDataSimulation_s* GetSimulationData(void);

/******************************************************************/
/* Task                                                           */
/******************************************************************/
uint8_t * sof_address;

/**
 * @brief      USB任务主函数
 * @param[in]  argument: 任务参数
 * @retval     None
 */
void usb_task(void const * argument)
{
    DataExchangeCenterPublish(&RECEIVE_ROBOT_CMD_DATA, ROBOT_CMD_DATA_NAME);
    DataExchangeCenterPublish(&USB_OFFLINE, USB_OFFLINE_NAME);
    // DataExchangeCenterPublish(&VIRTUAL_RC_CTRL, VIRTUAL_RC_NAME);

    MX_USB_DEVICE_Init();

    vTaskDelay(10);  //等待USB设备初始化完成
    UsbInit();

    while (1) {
        UsbSendData();
        UsbReceiveData();
        // GetCmdData();
        // GetVirtualRcCtrlData();
        GetRobotCmdData();
        GetSimulationData();
        GetVisionData();

        if (HAL_GetTick() - RECEIVE_TIME > USB_OFFLINE_THRESHOLD) {
            USB_OFFLINE = true;
            CONTINUE_RECEIVE_CNT = 0;
        } else if (CONTINUE_RECEIVE_CNT > USB_CONNECT_CNT) {
            USB_OFFLINE = false;
        } else {
            CONTINUE_RECEIVE_CNT++;
        }

        vTaskDelay(USB_TASK_CONTROL_TIME);

#if INCLUDE_uxTaskGetStackHighWaterMark
        usb_high_water = uxTaskGetStackHighWaterMark(NULL);
#endif
    }
}

/*******************************************************************************/
/* Main Function                                                               */
/*******************************************************************************/

/**
 * @brief      USB初始化
 * @param      None
 * @retval     None
 */
static void UsbInit(void)
{
    // 订阅数据
    solveAns = GetSolveAns();
    IMU = DataExchangeCenterSubscribe(IMU_NAME);                             // 获取IMU数据指针
    // FDB_SPEED_VECTOR = DataExchangeCenterSubscribe(CHASSIS_FDB_SPEED_NAME);  // 获取底盘速度矢量指针

    // 数据置零
    // memset(&LAST_SEND_TIME, 0, sizeof(LastSendTime_t));
    memset(&RECEIVE_ROBOT_CMD_DATA, 0, sizeof(ReceiveDataRobotCmd_s));
    // memset(&RECEIVE_PID_DEBUG_DATA, 0, sizeof(ReceiveDataPidDebug_s));
    // memset(&RECEIVE_VIRTUAL_RC_DATA, 0, sizeof(ReceiveDataVirtualRc_s));
    // memset(&ROBOT_CMD_DATA, 0, sizeof(RobotCmdData_t));
    // memset(&VIRTUAL_RC_CTRL, 0, sizeof(RC_ctrl_t));
    memset(&RECEIVE_VISION_DATA, 0, sizeof(ReceiveDataVision_s));
    memset(&RECEIVE_SIMULATION_DATA, 0, sizeof(ReceiveDataSimulation_s));

    // // 1.初始化调试数据包
    // // 帧头部分
    // SEND_DATA_DEBUG.frame_header.sof = SEND_SOF;
    // SEND_DATA_DEBUG.frame_header.len = (uint8_t)(sizeof(SendDataDebug_s) - 6);
    // SEND_DATA_DEBUG.frame_header.id = DEBUG_DATA_SEND_ID;
    // append_CRC8_check_sum(  // 添加帧头 CRC8 校验位
    //     (uint8_t *)(&SEND_DATA_DEBUG.frame_header), sizeof(SEND_DATA_DEBUG.frame_header));
    // // 数据部分
    // for (uint8_t i = 0; i < DEBUG_PACKAGE_NUM; i++) {
    //     SEND_DATA_DEBUG.packages[i].type = 1;
    //     SEND_DATA_DEBUG.packages[i].name[0] = '\0';
    // }

    // // 2.初始化IMU数据包
    // SEND_DATA_IMU.frame_header.sof = SEND_SOF;
    // SEND_DATA_IMU.frame_header.len = (uint8_t)(sizeof(SendDataImu_s) - 6);
    // SEND_DATA_IMU.frame_header.id = IMU_DATA_SEND_ID;
    // append_CRC8_check_sum(  // 添加帧头 CRC8 校验位
    //     (uint8_t *)(&SEND_DATA_IMU.frame_header), sizeof(SEND_DATA_IMU.frame_header));

    // 3.初始化机器人信息数据包
    // 帧头部分
    // SEND_DATA_ROBOT_INFO.header = SEND_SOF;
    SEND_DATA_ROBOT_INFO.id = ROBOT_INFO_DATA_SEND_ID;
    // 数据部分
    SEND_DATA_ROBOT_INFO.data.robot_id = 0;
    SEND_DATA_ROBOT_INFO.data.current_hp = 0;
    SEND_DATA_ROBOT_INFO.data.shooter_heat = 0;
    SEND_DATA_ROBOT_INFO.data.team_color = 0;
    SEND_DATA_ROBOT_INFO.data.is_attacked = 0;


    // // 4.初始化pid调参数据
    // SEND_DATA_PID.frame_header.sof = SEND_SOF;
    // SEND_DATA_PID.frame_header.len = (uint8_t)(sizeof(SendDataPidDebug_s) - 6);
    // SEND_DATA_PID.frame_header.id = PID_DEBUG_DATA_SEND_ID;
    // append_CRC8_check_sum(  // 添加帧头 CRC8 校验位
    //     (uint8_t *)(&SEND_DATA_PID.frame_header), sizeof(SEND_DATA_PID.frame_header));

    // 5.初始化所有机器人血量数据
    // SEND_DATA_ALL_ROBOT_HP.header = SEND_SOF;
    SEND_DATA_ALL_ROBOT_HP.id = ALL_ROBOT_HP_SEND_ID;

    // 6.初始化比赛状态数据
    // SEND_DATA_GAME_STATUS.header = SEND_SOF;
    SEND_DATA_GAME_STATUS.id = GAME_STATUS_SEND_ID;

    // // 7.初始化机器人运动数据
    // SEND_ROBOT_MOTION_DATA.frame_header.sof = SEND_SOF;
    // SEND_ROBOT_MOTION_DATA.frame_header.len = (uint8_t)(sizeof(SendDataRobotMotion_s) - 6);
    // SEND_ROBOT_MOTION_DATA.frame_header.id = ROBOT_MOTION_DATA_SEND_ID;
    // append_CRC8_check_sum(  // 添加帧头 CRC8 校验位
    //     (uint8_t *)(&SEND_ROBOT_MOTION_DATA.frame_header),
    //     sizeof(SEND_ROBOT_MOTION_DATA.frame_header));

    // 8.初始化视觉发送数据
    // SEND_DATA_VISION.header = SEND_SOF;
    SEND_DATA_VISION.id = VISION_SEND_ID;
}

/**
 * @brief      用USB发送数据
 * @param      None
 * @retval     None
 */
static void UsbSendData(void)
{
    // // 发送Imu数据
    // CheckDurationAndSend(Imu);
    // // 发送Debug数据
    // CheckDurationAndSend(Debug);
    // 发送RobotInfo数据
    CheckDurationAndSend(RobotInfo);
    // 发送AllRobotHp数据
    CheckDurationAndSend(AllRobotHp);
    // 发送GameStatus数据
    CheckDurationAndSend(GameStatus);
    // // 发送RobotMotion数据
    // CheckDurationAndSend(RobotMotion);
    // 发送Vision数据
    CheckDurationAndSend(Vision);
}

/**
 * @brief      USB接收数据
 * @param      None
 * @retval     None
 */
static void UsbReceiveData(void)
{
    static uint32_t len = USB_RECEIVE_LEN;
    static uint8_t * rx_data_start_address = USB_RX_BUF;  // 接收数据包时存放于缓存区的起始位置
    static uint8_t * rx_data_end_address;  // 接收数据包时存放于缓存区的结束位置
    uint8_t * sof_address = USB_RX_BUF;

    // 计算数据包的结束位置
    rx_data_end_address = rx_data_start_address + USB_RECEIVE_LEN;
    // 读取数据
    USB_Receive(rx_data_start_address, &len);  // Read data into the buffer

    while (sof_address <= rx_data_end_address) {  // 解析缓冲区中的所有数据包
        // 寻找帧头位置
        // uint32_t HEAD_SOF = (sof_address[0] << 24) | (sof_address[1] << 16) | (sof_address[2] << 8) | sof_address[3];
        // while (HEAD_SOF != RECEIVE_SOF && (sof_address <= rx_data_end_address)) {
        //     HEAD_SOF = (sof_address[0] << 24) | (sof_address[1] << 16) | (sof_address[2] << 8) | sof_address[3];
        //     sof_address++;
        // }
        while (*(sof_address) != RECEIVE_SOF && (sof_address <= rx_data_end_address)) {
            sof_address++;
        }
        // 判断是否超出接收数据范围
        if (sof_address > rx_data_end_address) {
            break;  // 退出循环
        }
        // 检查CRC8校验
        bool crc8_ok = verify_CRC8_check_sum(sof_address, sizeof(FrameHeader_t));//HEADER_SIZE);
        if (crc8_ok) {
            uint8_t data_len = sof_address[1];
            uint8_t data_id = sof_address[2];
            // 检查整包CRC16校验 4: header size, 2: crc16 size
            bool crc16_ok = verify_CRC16_check_sum(sof_address, 4 + data_len + 2);
            if (crc16_ok) {
                switch (data_id) {
                    case ROBOT_CMD_DATA_RECEIVE_ID: {
                        memcpy(&RECEIVE_ROBOT_CMD_DATA, sof_address, sizeof(ReceiveDataRobotCmd_s));
                    } break;
                    // case PID_DEBUG_DATA_RECEIVE_ID: {
                    //     memcpy(&RECEIVE_PID_DEBUG_DATA, sof_address, sizeof(ReceiveDataPidDebug_s));
                    // } break;
                    // case VIRTUAL_RC_DATA_RECEIVE_ID: {
                    //     memcpy(
                    //         &RECEIVE_VIRTUAL_RC_DATA, sof_address, sizeof(ReceiveDataVirtualRc_s));
                    // } break;
                    case VISION_RECEIVE_ID: {
                        memcpy(&RECEIVE_VISION_DATA, sof_address, sizeof(ReceiveDataVision_s));
                    } break;
                    case SIMULATION_RECEIVE_ID: {
                        memcpy(&RECEIVE_SIMULATION_DATA, sof_address, sizeof(ReceiveDataSimulation_s));
                    } break;
                    default:
                        break;
                }
                if (*((uint32_t *)(&sof_address[4])) > LATEST_RX_TIMESTAMP) {
                    LATEST_RX_TIMESTAMP = *((uint32_t *)(&sof_address[4]));
                    RECEIVE_TIME = HAL_GetTick();
                }
            }
            sof_address += (data_len + HEADER_SIZE + 2);
        } else {  //CRC8校验失败，移动到下一个字节
            sof_address++;
        }
    }
    // 更新下一次接收数据的起始位置
    if (sof_address > rx_data_start_address + USB_RECEIVE_LEN) {
        // 缓冲区中没有剩余数据，下次接收数据的起始位置为缓冲区的起始位置
        rx_data_start_address = USB_RX_BUF;
    } else {
        uint16_t remaining_data_len = USB_RECEIVE_LEN - (sof_address - rx_data_start_address);
        // 缓冲区中有剩余数据，下次接收数据的起始位置为缓冲区中剩余数据的起始位置
        rx_data_start_address = USB_RX_BUF + remaining_data_len;
        // 将剩余数据移到缓冲区的起始位置
        memcpy(USB_RX_BUF, sof_address, remaining_data_len);
    }
}
/*******************************************************************************/
/* Send Function                                                               */
/*******************************************************************************/

// /**
//  * @brief 发送IMU数据
//  * @param duration 发送周期
//  */
// static void UsbSendImuData(void)
// {
//     if (IMU == NULL) {
//         return;
//     }

//     SEND_DATA_IMU.time_stamp = HAL_GetTick();

//     SEND_DATA_IMU.data.yaw = IMU->yaw;
//     SEND_DATA_IMU.data.pitch = IMU->pitch;
//     SEND_DATA_IMU.data.roll = IMU->roll;

//     SEND_DATA_IMU.data.yaw_vel = IMU->yaw_vel;
//     SEND_DATA_IMU.data.pitch_vel = IMU->pitch_vel;
//     SEND_DATA_IMU.data.roll_vel = IMU->roll_vel;

//     append_CRC16_check_sum((uint8_t *)&SEND_DATA_IMU, sizeof(SendDataImu_s));
//     USB_Transmit((uint8_t *)&SEND_DATA_IMU, sizeof(SendDataImu_s));
// }

// /**
//  * @brief 发送DEBUG数据
//  * @param duration 发送周期
//  */
// static void UsbSendDebugData(void)
// {
//     SEND_DATA_DEBUG.time_stamp = HAL_GetTick();
//     append_CRC16_check_sum((uint8_t *)&SEND_DATA_DEBUG, sizeof(SendDataDebug_s));
//     USB_Transmit((uint8_t *)&SEND_DATA_DEBUG, sizeof(SendDataDebug_s));
// }

/**
 * @brief 发送机器人信息数据
 * @param duration 发送周期
 */
static void UsbSendRobotInfoData(void)
{
    get_robot_info_id(&SEND_DATA_ROBOT_INFO.data.robot_id);
    get_robot_info_hp(&SEND_DATA_ROBOT_INFO.data.current_hp);
    get_power_heat_data_mode(&SEND_DATA_ROBOT_INFO.data.shooter_heat,1);
    SEND_DATA_ROBOT_INFO.data.team_color = get_team_color();
    SEND_DATA_ROBOT_INFO.data.is_attacked = 0;

    append_CRC16_check_sum((uint8_t *)&SEND_DATA_ROBOT_INFO, sizeof(SendDataRobotInfo_s));
    USB_Transmit((uint8_t *)&SEND_DATA_ROBOT_INFO, sizeof(SendDataRobotInfo_s));
}

/**
 * @brief 发送全场机器人hp信息数据
 * @param duration 发送周期
 */
static void UsbSendAllRobotHpData(void)
{
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.red_1_robot_hp,1);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.red_2_robot_hp,2);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.red_3_robot_hp,3);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.red_4_robot_hp,4);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.red_5_robot_hp,5);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.red_7_robot_hp,7);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.red_base_hp,8);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.red_outpost_hp,9);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.blue_1_robot_hp,11);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.blue_2_robot_hp,12);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.blue_3_robot_hp,13);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.blue_4_robot_hp,14);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.blue_5_robot_hp,15);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.blue_7_robot_hp,17);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.blue_base_hp,18);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.blue_outpost_hp,19);

    append_CRC16_check_sum((uint8_t *)&SEND_DATA_ALL_ROBOT_HP, sizeof(SendDataAllRobotHp_s));
    USB_Transmit((uint8_t *)&SEND_DATA_ALL_ROBOT_HP, sizeof(SendDataAllRobotHp_s));
}

/**
 * @brief 发送比赛状态数据
 * @param duration 发送周期
 */
static void UsbSendGameStatusData(void)
{
    get_game_status_progress(&SEND_DATA_GAME_STATUS.data.game_progress);
    get_game_status_time(&SEND_DATA_GAME_STATUS.data.stage_remain_time);

    SEND_DATA_GAME_STATUS.data.is_canter_buff_down = 0;
    SEND_DATA_GAME_STATUS.data.center_buff_owner = 0;

    append_CRC16_check_sum((uint8_t *)&SEND_DATA_GAME_STATUS, sizeof(SendDataGameStatus_s));
    USB_Transmit((uint8_t *)&SEND_DATA_GAME_STATUS, sizeof(SendDataGameStatus_s));
}


 
static void UsbSendVisionData(void)
{
    
    SEND_DATA_VISION.time_stamp = 0;//HAL_GetTick();

    SEND_DATA_VISION.data.detect_color = get_detect_color();
    SEND_DATA_VISION.data.reset_tracker = 0;
    SEND_DATA_VISION.data.reserved = 0;
    
    // 对IMU数据进行四位小数处理
    SEND_DATA_VISION.data.roll  = IMU->roll;
    SEND_DATA_VISION.data.pitch = -IMU->pitch; 
    SEND_DATA_VISION.data.yaw   = IMU->yaw;

    SEND_DATA_VISION.data.aim_x = solveAns->aim_x;
    SEND_DATA_VISION.data.aim_y = solveAns->aim_y;
    SEND_DATA_VISION.data.aim_z = solveAns->aim_z;

    append_CRC16_check_sum((uint8_t *)&SEND_DATA_VISION, sizeof(SendDataVision_s));
    USB_Transmit((uint8_t *)&SEND_DATA_VISION, sizeof(SendDataVision_s));
}

/*******************************************************************************/
/* Receive Function                                                            */
/*******************************************************************************/

// static void GetCmdData(void)
// {
//     ROBOT_CMD_DATA.id = ROBOT_CMD_DATA_RECEIVE_ID;
//     ROBOT_CMD_DATA.data.stop_gimbal_scan
//     // ROBOT_CMD_DATA.speed_vector.vx = 0;RECEIVE_ROBOT_CMD_DATA.data.speed_vector.vx;
//     // ROBOT_CMD_DATA.speed_vector.vy = 0;RECEIVE_ROBOT_CMD_DATA.data.speed_vector.vy;
//     // ROBOT_CMD_DATA.speed_vector.wz = 0;RECEIVE_ROBOT_CMD_DATA.data.speed_vector.wz;

//     // ROBOT_CMD_DATA.chassis.yaw = 0;RECEIVE_ROBOT_CMD_DATA.data.chassis.yaw;
//     // ROBOT_CMD_DATA.chassis.pitch = RECEIVE_ROBOT_CMD_DATA.data.chassis.pitch;
//     // ROBOT_CMD_DATA.chassis.roll = RECEIVE_ROBOT_CMD_DATA.data.chassis.roll;
//     // ROBOT_CMD_DATA.chassis.leg_length = RECEIVE_ROBOT_CMD_DATA.data.chassis.leg_lenth;

//     // ROBOT_CMD_DATA.gimbal.yaw = RECEIVE_ROBOT_CMD_DATA.data.gimbal.yaw;
//     // ROBOT_CMD_DATA.gimbal.pitch = RECEIVE_ROBOT_CMD_DATA.data.gimbal.pitch;

//     // ROBOT_CMD_DATA.shoot.fire = RECEIVE_ROBOT_CMD_DATA.data.shoot.fire;
//     // ROBOT_CMD_DATA.shoot.fric_on = RECEIVE_ROBOT_CMD_DATA.data.shoot.fric_on;
// }

// static void GetVirtualRcCtrlData(void)
// {
//     memcpy(&VIRTUAL_RC_CTRL, &RECEIVE_VIRTUAL_RC_DATA.data, sizeof(RC_ctrl_t));
// }

ReceiveDataRobotCmd_s* GetRobotCmdData(void){
    return &RECEIVE_ROBOT_CMD_DATA;
}

ReceiveDataVision_s* GetVisionData(void){
    return &RECEIVE_VISION_DATA;
}

ReceiveDataSimulation_s* GetSimulationData(void){
    return &RECEIVE_SIMULATION_DATA;
}

/*******************************************************************************/
/* Public Function                                                             */
/*******************************************************************************/

/**
 * @brief 修改调试数据包
 * @param index 索引
 * @param data  发送数据
 * @param name  数据名称
 */
// void ModifyDebugDataPackage(uint8_t index, float data, const char * name)
// {
//     SEND_DATA_DEBUG.packages[index].data = data;

//     if (SEND_DATA_DEBUG.packages[index].name[0] != '\0') {
//         return;
//     }

//     uint8_t i = 0;
//     while (name[i] != '\0' && i < 10) {
//         SEND_DATA_DEBUG.packages[index].name[i] = name[i];
//         i++;
//     }

//     //TODO:添加对数据名称的一些检查工作
// }

/**
 * @brief 获取上位机控制指令：云台姿态，基于欧拉角 r×p×y
 * @param axis 轴id，可配合定义好的轴id宏 AX_PITCH,AX_YAW 使用
 * @return (rad) 云台姿态
 */
// inline float GetScCmdGimbalAngle(uint8_t axis)
// {
//     if (axis == AX_YAW) {
//         return ROBOT_CMD_DATA.gimbal.yaw;
//     } else if (axis == AX_PITCH) {
//         return ROBOT_CMD_DATA.gimbal.pitch;
//     }
//     return 0.0f;
// }
/*------------------------------ End of File ------------------------------*/


// //-----------------------------------------------------------------------------
// /**
//   ****************************(C) COPYRIGHT 2019 DJI****************************
//   * @file       usb_task.c/h
//   * @brief      usb outputs the error message.usbÊä³ö´íÎóÐÅÏ¢
//   * @note       
//   * @history
//   *  Version    Date            Author          Modification
//   *  V1.0.0     Nov-11-2019     RM              1. done
//   *
//   @verbatim
//   ==============================================================================

//   ==============================================================================
//   @endverbatim
//   ****************************(C) COPYRIGHT 2019 DJI****************************
//   */
// #include "usb_task.h"

// #include "cmsis_os.h"

// #include "usb_device.h"
// #include "usbd_cdc_if.h"
// #include <stdio.h>
// #include <stdarg.h>
// #include "string.h"

// #include "detect_task.h"
// #include "voltage_task.h"

// #include "referee.h"


// static void usb_printf(const char *fmt,...);

// static uint8_t usb_buf[256];
// static const char status[2][7] = {"OK", "ERROR!"};
// const error_t *error_list_usb_local;
// robot_status_t* robot_status;
// uint8_t color_robo; 

// void usb_task(void const * argument)
// {
//     robot_status = get_robot_status();
//     color_robo = get_team_color();

//     MX_USB_DEVICE_Init();
//     error_list_usb_local = get_error_list_point();


//     while(1)
//     {
//         osDelay(1000);
//         usb_printf("%x\r\n",robot_status->robot_id);
// //         usb_printf(
// // "******************************\r\n\
// // voltage percentage:%d%% \r\n\
// // DBUS:%s\r\n\
// // chassis motor1:%s\r\n\
// // chassis motor2:%s\r\n\
// // chassis motor3:%s\r\n\
// // chassis motor4:%s\r\n\
// // yaw motor:%s\r\n\
// // pitch motor:%s\r\n\
// // trigger motor:%s\r\n\
// // gyro sensor:%s\r\n\
// // accel sensor:%s\r\n\
// // mag sensor:%s\r\n\
// // referee usart:%s\r\n\
// // robot id:%x\r\n\
// // robot id:%c\r\n\
// // robot id:%d\r\n\
// // ******************************\r\n",
// //             get_battery_percentage(), 
// //             status[error_list_usb_local[DBUS_TOE].error_exist],
// //             status[error_list_usb_local[CHASSIS_MOTOR1_TOE].error_exist],
// //             status[error_list_usb_local[CHASSIS_MOTOR2_TOE].error_exist],
// //             status[error_list_usb_local[CHASSIS_MOTOR3_TOE].error_exist],
// //             status[error_list_usb_local[CHASSIS_MOTOR4_TOE].error_exist],
// //             status[error_list_usb_local[YAW_GIMBAL_MOTOR_TOE].error_exist],
// //             status[error_list_usb_local[PITCH_GIMBAL_MOTOR_TOE].error_exist],
// //             status[error_list_usb_local[TRIGGER_MOTOR_TOE].error_exist],
// //             status[error_list_usb_local[BOARD_GYRO_TOE].error_exist],
// //             status[error_list_usb_local[BOARD_ACCEL_TOE].error_exist],
// //             status[error_list_usb_local[BOARD_MAG_TOE].error_exist],
// //             status[error_list_usb_local[REFEREE_TOE].error_exist]),
// //             robot_status->robot_id,
// //             robot_status->robot_id,
// //             robot_status->robot_id;
//      }

// }

// static void usb_printf(const char *fmt,...)
// {
//     static va_list ap;
//     uint16_t len = 0;

//     va_start(ap, fmt);

//     len = vsprintf((char *)usb_buf, fmt, ap);

//     va_end(ap);


//     CDC_Transmit_FS(usb_buf, len);
// }
